#include "nr.h"

void atualiza_rho (int nelements,
                   int nelectrodes,
                   double **H,
                   double *v_rho,
                   double **regul_matriz,
                   double *V_calculado,
                   double *V_medido,
                   int *RtR_SIMPLIFY,
                   int *error_saida)
{
  int i,j,k;
  double **HtH;
  double aux1,aux2,erro_V;
  double *delta_v;
  double **delta_rho, **b,*V_medido_aux;
  double *aux,sum;
  FILE *fp;

  HtH=dmatrix(1,nelements,1,nelements);
  delta_v=dvector(1,nelectrodes*nelectrodes);
  V_medido_aux=dvector(1,nelectrodes*nelectrodes);
  delta_rho=dmatrix(1,nelements,1,2);
  b=dmatrix(1,nelements,1,2);
  aux=dvector(1,nelements);

  erro_V=0;
  for (k=1;k<=nelectrodes*nelectrodes;k++){
    V_medido_aux[k]=V_medido[k];
    if (RtR_SIMPLIFY[k]!= 0)
      delta_v[k]=V_medido[k]-V_calculado[k];
    else{
      for(j=1;j<=nelements;j++){
        H[k][j]=0.0;
      }
      delta_v[k]=0.0;
      V_medido_aux[k]=0.0;
      V_calculado[k]=0.0;
    }
    erro_V+=delta_v[k]*delta_v[k];
  }
  print_dvector(nelectrodes*nelectrodes,V_medido_aux,"V_medido.txt");
  print_dvector(nelectrodes*nelectrodes,V_calculado,"V_calculado.txt");
  
  for (i=1;i<=nelements;i++){
    sum=0.0;
    for (j=1;j<=nelements;j++){
      sum+=regul_matriz[i][j]*v_rho[j];
    }
    aux[i]=sum;
  }
  
  sum=0.0;
  for (i=1;i<=nelements;i++){
    sum+=v_rho[i]*aux[i];
  }
  
  
  fp=fopen("arquivo_erro_V_teste_rho.txt","a");
  fprintf(fp,"%1.20lf\n", sqrt(erro_V)+sum);
  fclose(fp);

  multiplytrans(H,
                HtH, 
                nelectrodes*nelectrodes,
                nelements,
                1);

  for (i=1;i<=nelements;i++){
    aux1=0.0;
    aux2=0.0;
    for (j=1;j<=nelements;j++){
      HtH[i][j]+=regul_matriz[i][j];
      aux2+=regul_matriz[i][j]*v_rho[j];
    }
    for (k=1;k<=nelectrodes*nelectrodes;k++){
      aux1+=H[k][i]*delta_v[k];
    }
    b[i][1]=aux1-aux2;
  }

  sist_lin_esparso_intel(nelements,
                         HtH,
                         1,
                         b,
                         delta_rho,
                         error_saida,
                         1);

  for (i=1;i<=nelements;i++){
    v_rho[i]+=0.1*delta_rho[i][1];
    if (v_rho[i] < 0.0)
      v_rho[i]=1.0;
  }
  
  print_dvector(nelements,v_rho,"teste_rho.txt");

  free_dmatrix(HtH,1,nelements,1,nelements);
  free_dvector(delta_v,1,nelectrodes*nelectrodes);
  free_dvector(V_medido_aux,1,nelectrodes*nelectrodes);
  free_dmatrix(delta_rho,1,nelements,1,2);
  free_dmatrix(b,1,nelements,1,2);
  free_dvector(aux,1,nelements);
}
